#include "jy901.h"

extern char usart5_receive [100] ;
extern unsigned char  uart5_receive_length;

void GetValue_FromJY901(float * value){

	int i = 0 ;
	int k = 0 ;
	
	if(uart5_receive_length==44){
		for (i = 0 ; i < 3 ; i++){
			short temp1 = usart5_receive[3+i*2] ;
			short temp2 = usart5_receive[2+i*2] ;
			short temp3 = temp1<<8|temp2;
			value[i] = (float)(temp3)*1.0/32768*16*9.8;
		}
		
		for (i = 0 ; i < 3 ; i++){
			short temp1 = usart5_receive[14+i*2] ;
			short temp2 = usart5_receive[13+i*2] ;
			short temp3 = temp1<<8|temp2;
			value[i+3] = (float)(temp3)*1.0/32768*2000;
		}
		
		for (i = 0 ; i < 3 ; i++){
			short temp1 = usart5_receive[25+i*2] ;
			short temp2 = usart5_receive[24+i*2] ;
			short temp3 = temp1<<8|temp2;
			value[i+6] = (float)(temp3)*1.0/32768*180;
		}
				
	}
	
}

static double P = 1.2;
static double I = 0;
static double D = 0.1;

int Position_PD_ABCD(int now, int set)
{
	static float Bias, Output, Last_Bias, inral;
	Bias = now - set;
	inral += Bias;
	Output = P * Bias + I * inral + D * (Bias - Last_Bias);
	Last_Bias = Bias;
	return Output;
}
